function [template optimCP optimAlpha] = atlasFormationSubstitution(atlasImages, controlPointPrior)
% atlasFormation: This function constructs a template image and also
% returns a set of momenta vectors that deform the template to each of the
% atlas images.
% Parameters:   atlasImages - the set of images in the atlas of a particular
%               class. For 2D images they have dimensions Ns*d1*d2
%               controlPointPrior - matrix of size D*ncp containing the
%               prior of the control point positions
% Returns:  templateImage - the template image of the dataset d1*d2
%           optimCP - the position of the control points after gradient
%           descent Ns*D*ncp. For 2D, D=2. i.e. N2*2*ncp
%           optimAlpha - the momenta vectors that deform template image to
%           each of the image in the atlas. For 2D Ns*2*ncp

%% if you do not want the debug stuff
verbose = 0;

%% registration an gradient descent parameters
para.sigmaK = 3;
% Always have this as 3 for digits database. Based upon the width of the
% contours. Never to be changed for any database!!
para.gamma = 0.1; % was 0.0001
para.stepfactorCP = 0.1;
para.stepfactorAlpha = 0.8;  % was 0.1
para.stepfactorTemplate = 0.8; % was 0.01
para.moveCtrlPts = 0;
% sparsity parameters
para.gammasp = 0.0;
para.Csp = 10^(-2);

para.maxIterations = 50;
para.maxTauIter = 50;
para.blurGamma = 0.1; % 0.1 for digits

% termination criterion breakratio
para.breakRatio = 10^(-4);

%% first initialization of the template is nothing but the mean image.
template = squeeze(mean(atlasImages,1));

%% preprocessing images
% if(strcmp(class(source),'uint8'))
%     source = double(source./255);
%     target = double(target./255);
% end
% 
template = gaussianBlur(template,para.blurGamma);
atlas = zeros(size(atlasImages));
Ns = size(atlasImages,1);
for sample = 1:Ns
 atlas(sample,:,:) = gaussianBlur(squeeze(atlasImages(sample,:,:)),para.blurGamma);
end

%%
% initialise control points on a regular grid and the momenta vectors to
% zero.
[nrow ncol] = size(template);
% sample on a regular 5x5 grid
%noGridPts = 5;
%[x y] = meshgrid(ncol/(noGridPts+1):ncol/(noGridPts+1):ncol-1,nrow/(noGridPts+1):nrow/(noGridPts+1):nrow-1);
%[x y] = meshgrid(para.sigmaK:para.sigmaK:ncol-1,para.sigmaK:para.sigmaK:nrow-1);

%[x y] = meshgrid(2.5:3:16,2.5:3:16);
% to be changed - need to wait for the results of the new data.

x = controlPointPrior(1,:); y = controlPointPrior(2,:);
% x = [2 2 4 4 4 6 6 6 6 7 8 9 9 9 10 11 12 12 12 14 15 15]';
% y = [7 12 8 10 13 3 6 12 14 9 3 7 11 14 5 10 3 8 13 9 3 12]';

cp = zeros(Ns, size([x(:),y(:)]',1), size([x(:),y(:)]',2));


for sample = 1:Ns
    cp(sample,:,:) = [x(:), y(:)]';
end
alpha = zeros(size(cp));
    
CP = zeros([size(cp),para.maxIterations]);
ALPHA = zeros([size(alpha),para.maxIterations]);

CP(:,:,:,1) = cp;
ALPHA(:,:,:,1) = alpha;
% perform the gradient descent
[Eprev iDiff regularizer sparsity] = EnergyAtlas(CP(:,:,:,1),ALPHA(:,:,:,1),template,atlas,para);
E0 = Eprev;

tautemplate = para.stepfactorTemplate;
taucp = para.stepfactorCP;
taualpha = para.stepfactorAlpha;
for iter = 1:para.maxIterations
    [Eprev iDiff regularizer sparsity] = EnergyAtlas(CP(:,:,:,iter),ALPHA(:,:,:,iter),template,atlas,para);
    Enow = Eprev;
    Elast = Enow;
    tauIter = 0;    
    GRADALPHA = gradAlphaAtlas(CP(:,:,:,iter),ALPHA(:,:,:,iter),template,atlas,para);
    if(para.moveCtrlPts==1)
        GRADCP = gradCPAtlas(CP(:,:,:,iter),ALPHA(:,:,:,iter),template,atlas,para);
    end
    while((Eprev<=Enow)&&(tauIter<para.maxTauIter))
        tauIter = tauIter + 1;
        % adaptive stepsize for the control points and alpha vectors term
        % with motivation from the energy term        
        ALPHA(:,:,:,iter+1) = ALPHA(:,:,:,iter) - taualpha.*GRADALPHA;
        if(para.moveCtrlPts==1)            
            CP(:,:,:,iter+1) = CP(:,:,:,iter) - taucp.*GRADCP;
        else
            CP(:,:,:,iter+1) = CP(:,:,:,iter);
        end
        [Enow iDiff regularizer sparsity] = EnergyAtlas(CP(:,:,:,iter+1),ALPHA(:,:,:,iter+1),template,atlas,para);
        taucp = taucp/2; taualpha = taualpha/2;
        %disp(['TauCP = ',num2str(taucp), ' EnergyNow = ', num2str(Enow), ' EnergyOld = ', num2str(Eprev)]);
%         if((tauIter == para.maxTauIter)&&(Eprev<=Enow))
%             ALPHA(:,:,:,iter+1) = ALPHA(:,:,:,iter);
%             CP(:,:,:,iter+1) = CP(:,:,:,iter);
%         end
    end
    if(verbose == 1)
        disp(['tauIter: ', num2str(tauIter), ' tauAlpha: ', num2str(taualpha)]);
    end
    if(tauIter==1)
        taucp = 8*taucp; taualpha = 8*taualpha;
    end
    
    tauIter = 0;
    
    %Enow = Eprev;
    Eprev = Enow;
    
%%    
    GRADTEMPLATE = gradTemplateAtlas(CP(:,:,:,iter),ALPHA(:,:,:,iter),template,atlas,para);
    while((Eprev<=Enow)&&(tauIter<para.maxTauIter))
        tauIter = tauIter + 1;
        % adaptive stepsize for the control points and alpha vectors term
        % with motivation from the energy term        
        templateTemp = template - tautemplate.*GRADTEMPLATE;
        [Enow iDiff regularizer sparsity] = EnergyAtlas(CP(:,:,:,iter+1),ALPHA(:,:,:,iter+1),templateTemp,atlas,para);
        tautemplate = tautemplate/2;
        %disp(['TauCP = ',num2str(taucp), ' EnergyNow = ', num2str(Enow), ' EnergyOld = ', num2str(Eprev)]);
%         if((tauIter == para.maxTauIter)&&(Eprev<=Enow))
%             ALPHA(:,:,:,iter+1) = ALPHA(:,:,:,iter);
%             CP(:,:,:,iter+1) = CP(:,:,:,iter);
%         end
    end
    if(verbose == 1)
        disp(['tauIter: ', num2str(tauIter), ' tautemplate: ', num2str(tautemplate)]);
    end
    template = templateTemp;
    if(tauIter==1)
        tautemplate = 8*tautemplate;
    end


    % now we need to pull back all the subject images into the template
    % space and average them all.    
%     [template deformedTemplate] = averagedPullbackSubjects(CP(:,:,:,iter+1),ALPHA(:,:,:,iter+1),atlas,para);

%%    
    if(verbose == 1)
        disp([num2str(iter),'. Energy = ',num2str(Enow),' ImageDiff = ',num2str(iDiff),' Regularity = ',num2str(regularizer), ' Sparsity = ',num2str(sparsity)]);
    end
    % the following 4 variables are only for display of the energy graph
    Eprevtest(iter) = Enow;
    Difftest(iter) = iDiff;
    Regularizertest(iter) = regularizer;
    Sparsitytest(iter) = sparsity;
        
    % updating the values of the returned parameters
    %deformedSrc = displayDeformedImage(CP(:,:,iter+1),ALPHA(:,:,iter+1), source, target, para);
    %imagesc(template); colormap gray;
    optimCP = CP(:,:,:,iter+1);
    optimAlpha = ALPHA(:,:,:,iter+1);

    if(verbose == 1)
    subplot(2,5,1); imagesc(squeeze(atlas(1,:,:))); colormap gray; hold on; quiver(squeeze(optimCP(1,1,:)),squeeze(optimCP(1,2,:)),squeeze(optimAlpha(1,1,:)),squeeze(optimAlpha(1,2,:)), 'Color', 'r');
    subplot(2,5,2); imagesc(squeeze(atlas(2,:,:))); hold on; quiver(squeeze(optimCP(2,1,:)),squeeze(optimCP(2,2,:)),squeeze(optimAlpha(2,1,:)),squeeze(optimAlpha(2,2,:)), 'Color', 'g');
    subplot(2,5,3); imagesc(squeeze(atlas(3,:,:))); hold on; quiver(squeeze(optimCP(3,1,:)),squeeze(optimCP(3,2,:)),squeeze(optimAlpha(3,1,:)),squeeze(optimAlpha(3,2,:)), 'Color', 'b');
    
    subplot(2,5,4); imagesc(squeeze(atlas(4,:,:))); hold on; quiver(squeeze(optimCP(4,1,:)),squeeze(optimCP(4,2,:)),squeeze(optimAlpha(4,1,:)),squeeze(optimAlpha(4,2,:)), 'Color', 'r');
    subplot(2,5,5); imagesc(squeeze(atlas(5,:,:))); hold on; quiver(squeeze(optimCP(5,1,:)),squeeze(optimCP(5,2,:)),squeeze(optimAlpha(5,1,:)),squeeze(optimAlpha(5,2,:)), 'Color', 'g');
    subplot(2,5,6); imagesc(squeeze(atlas(6,:,:))); hold on; quiver(squeeze(optimCP(6,1,:)),squeeze(optimCP(6,2,:)),squeeze(optimAlpha(6,1,:)),squeeze(optimAlpha(6,2,:)), 'Color', 'b');

    subplot(2,5,7); imagesc(squeeze(atlas(7,:,:))); hold on; quiver(squeeze(optimCP(7,1,:)),squeeze(optimCP(7,2,:)),squeeze(optimAlpha(7,1,:)),squeeze(optimAlpha(7,2,:)), 'Color', 'r');
    subplot(2,5,8); imagesc(squeeze(atlas(8,:,:))); hold on; quiver(squeeze(optimCP(8,1,:)),squeeze(optimCP(8,2,:)),squeeze(optimAlpha(8,1,:)),squeeze(optimAlpha(8,2,:)), 'Color', 'g');
    subplot(2,5,9); imagesc(squeeze(atlas(9,:,:))); hold on; quiver(squeeze(optimCP(9,1,:)),squeeze(optimCP(9,2,:)),squeeze(optimAlpha(9,1,:)),squeeze(optimAlpha(9,2,:)), 'Color', 'b');

    
%     subplot(2,5,1); imagesc(squeeze(deformedTemplate(1,:,:))); colormap gray; hold on; quiver(squeeze(optimCP(1,1,:)),squeeze(optimCP(1,2,:)),squeeze(optimAlpha(1,1,:)),squeeze(optimAlpha(1,2,:)), 'Color', 'r');
%     subplot(2,5,2); imagesc(squeeze(deformedTemplate(2,:,:))); hold on; quiver(squeeze(optimCP(2,1,:)),squeeze(optimCP(2,2,:)),squeeze(optimAlpha(2,1,:)),squeeze(optimAlpha(2,2,:)), 'Color', 'g');
%     subplot(2,5,3); imagesc(squeeze(deformedTemplate(3,:,:))); hold on; quiver(squeeze(optimCP(3,1,:)),squeeze(optimCP(3,2,:)),squeeze(optimAlpha(3,1,:)),squeeze(optimAlpha(3,2,:)), 'Color', 'b');
%     
%     subplot(2,5,4); imagesc(squeeze(deformedTemplate(4,:,:))); hold on; quiver(squeeze(optimCP(4,1,:)),squeeze(optimCP(4,2,:)),squeeze(optimAlpha(4,1,:)),squeeze(optimAlpha(4,2,:)), 'Color', 'r');
%     subplot(2,5,5); imagesc(squeeze(deformedTemplate(5,:,:))); hold on; quiver(squeeze(optimCP(5,1,:)),squeeze(optimCP(5,2,:)),squeeze(optimAlpha(5,1,:)),squeeze(optimAlpha(5,2,:)), 'Color', 'g');
%     subplot(2,5,6); imagesc(squeeze(deformedTemplate(6,:,:))); hold on; quiver(squeeze(optimCP(6,1,:)),squeeze(optimCP(6,2,:)),squeeze(optimAlpha(6,1,:)),squeeze(optimAlpha(6,2,:)), 'Color', 'b');
% 
%     subplot(2,5,7); imagesc(squeeze(deformedTemplate(7,:,:))); hold on; quiver(squeeze(optimCP(7,1,:)),squeeze(optimCP(7,2,:)),squeeze(optimAlpha(7,1,:)),squeeze(optimAlpha(7,2,:)), 'Color', 'r');
%     subplot(2,5,8); imagesc(squeeze(deformedTemplate(8,:,:))); hold on; quiver(squeeze(optimCP(8,1,:)),squeeze(optimCP(8,2,:)),squeeze(optimAlpha(8,1,:)),squeeze(optimAlpha(8,2,:)), 'Color', 'g');
%     subplot(2,5,9); imagesc(squeeze(deformedTemplate(9,:,:))); hold on; quiver(squeeze(optimCP(9,1,:)),squeeze(optimCP(9,2,:)),squeeze(optimAlpha(9,1,:)),squeeze(optimAlpha(9,2,:)), 'Color', 'b');
    
    subplot(2,5,10); imagesc(template);
    %imagesc(template); colormap(gray);
    end
    
    % for displaying the deformed image at the current point    
    %imagesc(deformedSrc); colormap gray;
    %hold on;
    %quiver(CP(1,:,iter+1), CP(2,:,iter+1), ALPHA(1,:,iter+1), ALPHA(2,:,iter+1));
    %hold off;
	[Enow iDiff regularizer sparsity] = EnergyAtlas(CP(:,:,:,iter+1),ALPHA(:,:,:,iter+1),template,atlas,para);
    %disp(['Breakratio: ',num2str((Eprev-Enow)/(E0-Enow))]);
    if(verbose == 1)
        disp(['Breakratio: ',num2str((Elast-Enow)/(E0-Enow))]);
    end
    %if(((Eprev-Enow)>=0)&&(iter>10))
    %    if(((Eprev-Enow)/(E0-Enow))<=para.breakRatio)
    if(((Elast-Enow)>=0)&&(iter>10))
        if(((Elast-Enow)/(E0-Enow))<=para.breakRatio)
            break;
        end
    end        
    pause(0.01);
    %imwrite(template,['/home/sci/tirpankar/fromStanley/imageCP/tempates/Template_',num2str(iter)],'png');
end
if(verbose == 1)
    figure; plot(Eprevtest,'Color','red'); hold on;
    plot(Difftest,'Color','green'); 
    plot(Regularizertest,'Color','blue'); 
    % plot(Sparsitytest,'Color','cyan');
    legend('Energy','imageDifference',['regularization with Gamma = ',num2str(para.gamma)], ...
    ['sparsity with GammaSP = ',num2str(para.gammasp),' C = ',num2str(para.Csp)],'Location','NorthEastOutside');
end

end

function val = gaussianKer(c1, c2, para)
% this function is a simple gaussian kernel which takes the co-ordinates of
% 2 points as an input and returns the scalar which is the measure of the
% similarity between 2 points computed from the L2 norm of their difference    
    val = exp(-sum((c1-c2).^2)./(para.sigmaK^2));
end

function val = velocityField(x,alpha,cp,para)
val = zeros(2,size(x,2));
for i = 1:size(cp,2)
    val = val + repmat(gaussianKer(x,repmat(cp(:,i),1,size(x,2)),para),2,1).*repmat(alpha(:,i),1,size(x,2));
end
end

function [template deformedTemplate] = averagedPullbackSubjects(cp, alpha, atlas, para)
    template = zeros(size(squeeze(atlas(1,:,:))));
    deformedTemplate = zeros(size(atlas));
    for sample = 1:size(atlas,1)
        deformedTemplate(sample,:,:) = pullBackTemplateSpace(squeeze(cp(sample,:,:)),squeeze(alpha(sample,:,:)),squeeze(atlas(sample,:,:)),para);
        template = template + squeeze(deformedTemplate(sample,:,:));
    end
    template = template./size(atlas,1);
end

function I0 = pullBackTemplateSpace(cp, alpha, Itar, para)
    [X Y] = meshgrid(1:size(Itar,2), 1:size(Itar,1));
    xk = [X(:), Y(:)]';
    vxk = velocityField(xk,alpha,cp,para);
    Xvk = reshape(vxk(1,:),size(Itar,1),size(Itar,2));
    Yvk = reshape(vxk(2,:),size(Itar,1),size(Itar,2));
    % probably wrong
    I0 = interp2(X,Y,Itar,X+Xvk,Y+Yvk);
    I0(isnan(I0)) = 0;
end

function val = gradTemplateAtlas(cp, alpha, template, atlas, para)
    % Do this by summing the gradient of the Image difference term of the
    % template with respect to each of the atlas images and then summing up
    % the gradients
    val = zeros(size(template));
    for sample = 1:size(atlas,1)
        val = val + gradTemplate(squeeze(cp(sample,:,:)),squeeze(alpha(sample,:,:)),template,squeeze(atlas(sample,:,:)),para);
    end    
end

function val = gradTemplate(cp, alpha, Isrc, Itar, para)
    % compute the residual image
    [X Y] = meshgrid(1:size(Isrc,2), 1:size(Isrc,1));
    yk = [X(:), Y(:)]';
    vyk = velocityField(yk,alpha,cp,para);
    Xvk = reshape(vyk(1,:),size(Isrc,1),size(Isrc,2));
    Yvk = reshape(vyk(2,:),size(Isrc,1),size(Isrc,2));
    % get the new deformed source I1 which is Isrc(yk-vyk)
    I1 = interp2(X,Y,Isrc,X-Xvk,Y-Yvk);
    I1(isnan(I1)) = 0;    
    Iresidual = (I1 - Itar);
    % splat the residual image into the template image domain
    val = splat(Iresidual, X-Xvk, Y-Yvk);
end

function val = splat(Isub, Xfield, Yfield)
    % this function splats the image Isub to the grid locations where Isub
    % is defined at the points (X,Y)
    % now that we have the weights for each of the terms, distribute each
    % of the Isub pixels to the neighbours
    maxX = size(Isub,1);
    maxY = size(Isub,2);
    val = zeros(size(Isub));
    for i = 1:size(Isub,1)
        for j = 1:size(Isub,2)
            X = Xfield(i,j); Y = Yfield(i,j);
            Xt = floor(X); Xb = ceil(X);
            Yl = floor(Y); Yr = ceil(Y);
            if(Xt==Xb)
                if(Yl==Yr)
                    Wa = 1;
                    Wb = 0; Wc = 0; Wd = 0;
                else
                    Wa = (Yr-Y)/(Yr-Yl);
                    Wb = (Y-Yl)/(Yr-Yl);
                    Wc = 0; Wd = 0;
                end
            else
                if(Yl==Yr)
                    Wa = (Xb-X)/(Xb-Xt);
                    Wc = (X-Xt)/(Xb-Xt);
                    Wb = 0; Wd = 0;
                else
                    Wa = ((Xb-X)/(Xb-Xt))*((Yr-Y)/(Yr-Yl));
                    Wb = ((Xb-X)/(Xb-Xt))*((Y-Yl)/(Yr-Yl));
                    Wc = ((X-Xt)/(Xb-Xt))*((Yr-Y)/(Yr-Yl));
                    Wd = ((X-Xt)/(Xb-Xt))*((Y-Yl)/(Yr-Yl));
                end
            end
            
            if((Xt>=1)&&(Yl>=1)&&(Xt<=maxX)&&(Yl<=maxY))
                val(Xt,Yl) = val(Xt,Yl) + Wa*Isub(i,j);
            end
            if((Xt>=1)&&(Yr>=1)&&(Xt<=maxX)&&(Yr<=maxY))
                val(Xt,Yr) = val(Xt,Yr) + Wb*Isub(i,j);
            end
            if((Xb>=1)&&(Yl>=1)&&(Xb<=maxX)&&(Yl<=maxY))
                val(Xb,Yl) = val(Xb,Yl) + Wc*Isub(i,j);
            end
            if((Xb>=1)&&(Yr>=1)&&(Xb<=maxX)&&(Yr<=maxY))
                val(Xb,Yr) = val(Xb,Yr) + Wd*Isub(i,j);
            end
        end
    end
    val = val';
end

function val = gradAlphaAtlas(cp, alpha, template, atlas, para)
    % Do this by computing the gradient registering the template image with
    % each of the images in the atlas.
    val = zeros(size(alpha));
    for sample = 1:size(atlas,1)
        val(sample,:,:) = gradAlpha(squeeze(cp(sample,:,:)),squeeze(alpha(sample,:,:)),template,squeeze(atlas(sample,:,:)),para);
    end
end

function val = gradAlpha(cp, alpha, Isrc, Itar, para)
    % first compute the matrix containing the position of the deformed grid
    % points from the target to the source. We are computing the term
    % yk - v(yk)
    % first define the regular grid
    [X Y] = meshgrid(1:size(Isrc,2), 1:size(Isrc,1));
    yk = [X(:), Y(:)]';
    vyk = velocityField(yk,alpha,cp,para);
    Xvk = reshape(vyk(1,:),size(Isrc,1),size(Isrc,2));
    Yvk = reshape(vyk(2,:),size(Isrc,1),size(Isrc,2));
    % now is the fun part in which you get the new deformed source I1 which
    % is Isrc(yk-vyk)
    I1 = interp2(X,Y,Isrc,X-Xvk,Y-Yvk);
    I1(isnan(I1)) = 0;
    [I1gradX I1gradY] = gradient(I1);
    Idiff = I1 - Itar;    
    Idiff = Idiff(:)'; I1grad = [I1gradX(:), I1gradY(:)]';
    val = zeros(2,size(alpha,2));
    % the following 2 loops are designed to run for all the control points
    for j = 1:size(alpha,2)
        % term2 = gaussianKer(cp, repmat(cp(:,j),1,size(cp,2)),para);
        term2 = alpha(:,j)*gaussianKer(cp, repmat(cp(:,j),1,size(cp,2)),para);
        % added the new sparsity penalty term
        if(sqrt(sum(alpha(:,j).^2))>=para.Csp)
            term3 = repmat(alpha(:,j)./sqrt(sum(alpha(:,j).^2)),1,size(alpha,2));
        else
            term3 = zeros(size(alpha));
        end
        val = val + para.gamma.*term2 + para.gammasp.*term3;
    end
    for k = 1:size(yk,2)
        %term1 = gaussianKer(cp, repmat(yk(:,k),1,size(cp,2)),para);
        term1 = I1grad(:,k)*(gaussianKer(cp, repmat(yk(:,k),1,size(cp,2)),para)*Idiff(:,k));
        val = val - term1;
    end
end

function val = gradCPAtlas(cp, alpha, template, atlas, para)
    val = zeros(size(cp));
    sumGrads = zeros([size(cp,2) size(cp,3)]);
    for sample = 1:size(atlas,1)
        sumGrads = sumGrads + gradCP(squeeze(cp(sample,:,:)),squeeze(alpha(sample,:,:)),template,squeeze(atlas(sample,:,:)),para);
    end
    for sample = 1:size(atlas,1)
        val(sample,:,:) = sumGrads;
    end
end

function val = gradCP(cp, alpha, Isrc, Itar, para)
    % first compute the matrix containing the position of the deformed grid
    % points from the target to the source. We are computing the term
    % yk - v(yk)
    % first define the regular grid
    [X Y] = meshgrid(1:size(Isrc,2), 1:size(Isrc,1));
    yk = [X(:), Y(:)]';
    vyk = velocityField(yk,alpha,cp,para);
    Xvk = reshape(vyk(1,:),size(Isrc,1),size(Isrc,2));
    Yvk = reshape(vyk(2,:),size(Isrc,1),size(Isrc,2));
    % now is the fun part in which you get the new deformed source I1 which
    % is Isrc(yk-vyk)
    I1 = interp2(X,Y,Isrc,X-Xvk,Y-Yvk);
    I1(isnan(I1)) = 0;
    [I1gradX I1gradY] = gradient(I1);
    Idiff = I1 - Itar;    
    Idiff = Idiff(:)'; I1grad = [I1gradX(:), I1gradY(:)]';
    val = zeros(2,size(cp,2));
    % the following 2 loops are designed to run for all the control points
    for k = 1:size(yk,2)
        term1 = gaussianKer(cp, repmat(yk(:,k),1,size(cp,2)),para);
        term1 = (I1grad(:,k)'*alpha).*(term1.*Idiff(:,k));
        term1 = repmat(term1,2,1).*(cp - repmat(yk(:,k),1,size(cp,2)));
        val = val + term1;
    end
    for j = 1:size(alpha,2)
        term2 = gaussianKer(cp, repmat(cp(:,j),1,size(cp,2)),para);
        term2 = ((alpha'*alpha(:,j))'.*term2);
        term2 = repmat(term2,2,1).*(cp - repmat(cp(:,j),1,size(cp,2)));
        val = val - para.gamma.*term2;
    end
    val = val.*(2/para.sigmaK^2);
end

function [Enow Imagediff Regularizer Sparsity] = EnergyAtlas(cp,alpha,template,atlas,para)
    Enow = 0; Imagediff = 0; Regularizer = 0; Sparsity = 0;
    for sample = 1:size(atlas,1)
        [E As Ls Sp] = Energy(squeeze(cp(sample,:,:)),squeeze(alpha(sample,:,:)),template,squeeze(atlas(sample,:,:)),para);
        Enow = Enow + E;
        Imagediff = Imagediff + As;
        Regularizer = Regularizer + Ls;
        Sparsity = Sparsity + Sp;
    end
end

function [val ImageDiff Regularizer Sparsity] = Energy(cp, alpha, Isrc, Itar, para)
    % first compute the matrix containing the position of the deformed grid
    % points from the target to the source. We are computing the term
    % yk - v(yk)
    % first define the regular grid
    [X Y] = meshgrid(1:size(Isrc,2), 1:size(Isrc,1));
    yk = [X(:), Y(:)]';
    vyk = velocityField(yk,alpha,cp,para);
    Xvk = reshape(vyk(1,:),size(Isrc,1),size(Isrc,2));
    Yvk = reshape(vyk(2,:),size(Isrc,1),size(Isrc,2));
    % now is the fun part in which you get the new deformed source I1 which
    % is Isrc(yk-vyk)
    I1 = interp2(X,Y,Isrc,X-Xvk,Y-Yvk);
    
    % the pixels outside the image all have a value of 0
    I1(isnan(I1)) = 0;
    
    Idiff = I1 - Itar;
    term1 = sum(Idiff(:).^2);
    val = zeros(1,size(alpha,2));
    term3 = 0;
    for j = 1:size(alpha,2)
        term2 = gaussianKer(cp, repmat(cp(:,j),1,size(cp,2)),para);
        term2 = ((alpha'*alpha(:,j))'.*term2);
        val = val + term2;
        term3 = term3 + LogC(sqrt(sum(alpha(:,j).^2)), para.Csp);
    end
    term2 = sum(val);    
    
    ImageDiff = term1;
    Regularizer = term2;
    Sparsity = term3;
    val = term1 + para.gamma*term2 + para.gammasp*term3;
end

function [val] = LogC(x,C)
    val = max(log(x), log(C)) - log(C);
end

function [G] = gaussianBlur(I,sigma)
H = zeros(size(I));
for i = 1:size(I,1)
    for j = 1:size(I,2)
        H(i,j) = exp(-(i^2+j^2)/sigma^2);
    end
end
H = fft2(H);
F = fft2(I,size(I,1),size(I,2));
G = real(ifft2(H.*F));
G = (G - min(G(:)))/(max(G(:)) - min(G(:)));
end